/**
 *
 */
package jp.seraph.jsmf.motion;

import java.util.Map.Entry;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jspf.pose.Pose;

/**
 *
 *
 */
public abstract class AbstractMotionElement implements MotionElement {
    public AbstractMotionElement(String aName, String aConditionElementName, int aStartPercentage){
        mStartPercentage = aStartPercentage;
        mName = aName;
        mConditionElementName = aConditionElementName;
    }

    private int mStartPercentage;
    private String mConditionElementName;
    private String mName;

    /**
     *
     * @see jp.seraph.jsmf.motion.MotionElement#getCurrentPercentage(jp.seraph.jspf.pose.Pose,
     *      jp.seraph.jspf.pose.Pose)
     */
    public int getCurrentPercentage(Pose aBasePose, Pose aCurrentPose) {
        Pose tGoal = this.getPoseElement().getPose();
        Pose tCurrent = aCurrentPose;
        Pose tBase = aBasePose;
        double tTotalDiff = 0.0;
        double tCurrentDiff = 0.0;

        for (Entry<JointIdentifier, Angle> tEntry : tGoal) {
            Angle tBaseAngle = tBase.getJointAngle(tEntry.getKey());
            Angle tCurrentAngle = tCurrent.getJointAngle(tEntry.getKey());
            Angle tGoalAngle = tGoal.getJointAngle(tEntry.getKey());

            tTotalDiff += tGoalAngle.subtract(tBaseAngle).abs().getDegree();

            double tDiff = tGoalAngle.subtract(tCurrentAngle).abs().getDegree();
            if (tDiff >= 1.0)
                tCurrentDiff += tDiff;
        }

        return (int) (((tTotalDiff - tCurrentDiff) / tTotalDiff) * 100);
    }

    /**
     *
     * @see jp.seraph.jsmf.motion.MotionElement#getStartPercentage()
     */
    public int getStartPercentage() {
        return mStartPercentage;
    }

    /**
     *
     * @see jp.seraph.jsmf.motion.MotionElement#getElementName()
     */
    public String getName() {
        return mName;
    }

    /**
     *
     * @see jp.seraph.jsmf.motion.MotionElement#getConditionElementName()
     */
    public String getConditionElementName() {
        return mConditionElementName;
    }

    /**
     *
     * @see jp.seraph.jsmf.motion.MotionElement#toTask(jp.seraph.jsmf.motion.SessionTask)
     */
    public ElementTask toTask(SessionTask aParentTask) {
        return new ElementTask(this, aParentTask);
    }
}
